#include "fedtime.hh"
#include "HwFederateAmbassador.hh"

#include <BridgeHelper.h>
#include <iostream>
#include "Utils.h"
#include "SumoSimulation.h"

using namespace std;

extern RTI::InteractionClassHandle hSumoClass;
extern RTI::ParameterHandle hSumoId;
extern RTI::ParameterHandle hSumoSpeed;
extern RTI::ParameterHandle hSumoAcceleration;
extern RTI::ParameterHandle hSumoX;
extern RTI::ParameterHandle hSumoY;
extern RTI::ParameterHandle hSumoZ;

extern RTI::InteractionClassHandle hSyncClass;
extern RTI::ParameterHandle hInfoNum;

extern RTIfedTime g_currentTime;
extern bool g_bConstrained;
extern bool g_bRegulation;
extern bool g_granted;

extern SumoSimulation *sumoSimulation;

int receiveInfoNum = 0;
int receiveInfoNumCount = 0;

HwFederateAmbassador::HwFederateAmbassador() {
}

HwFederateAmbassador::~HwFederateAmbassador() throw(RTI::FederateInternalError) {
}

////////////////////////////////////
// Federation Management Services //
////////////////////////////////////

void HwFederateAmbassador::synchronizationPointRegistrationSucceeded(
    const char *label) // supplied C4)
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::synchronizationPointRegistrationFailed(
    const char *label) // supplied C4)
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::announceSynchronizationPoint(
    const char *label, // supplied C4
    const char *tag) // supplied C4
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationSynchronized(
    const char *label) // supplied C4)
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::initiateFederateSave(
    const char *label) // supplied C4
throw(
    RTI::UnableToPerformSave,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationSaved() throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationNotSaved() throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreSucceeded(
    const char *label) // supplied C4
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreFailed(
    const char *label) // supplied C4
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestFederationRestoreFailed(
    const char *label,
    const char *reason) // supplied C4
throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationRestoreBegun() throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::initiateFederateRestore(
    const char *label, // supplied C4
    RTI::FederateHandle handle) // supplied C1
throw(
    RTI::SpecifiedSaveLabelDoesNotExist,
    RTI::CouldNotRestore,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationRestored() throw(
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::federationNotRestored() throw(
    RTI::FederateInternalError) {
}

/////////////////////////////////////
// Declaration Management Services //
/////////////////////////////////////

void HwFederateAmbassador::startRegistrationForObjectClass(
    RTI::ObjectClassHandle theClass) // supplied C1
throw(
    RTI::ObjectClassNotPublished,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::stopRegistrationForObjectClass(
    RTI::ObjectClassHandle theClass) // supplied C1
throw(
    RTI::ObjectClassNotPublished,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnInteractionsOn(
    RTI::InteractionClassHandle theHandle) // supplied C1
throw(
    RTI::InteractionClassNotPublished,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnInteractionsOff(
    RTI::InteractionClassHandle theHandle) // supplied C1
throw(
    RTI::InteractionClassNotPublished,
    RTI::FederateInternalError) {
}

////////////////////////////////
// Object Management Services //
////////////////////////////////

void HwFederateAmbassador::discoverObjectInstance(
    RTI::ObjectHandle theObject, // supplied C1
    RTI::ObjectClassHandle theObjectClass, // supplied C1
    const char *theObjectName) // supplied C4
throw(
    RTI::CouldNotDiscover,
    RTI::ObjectClassNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::reflectAttributeValues(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleValuePairSet &theAttributes, // supplied C4
    const RTI::FedTime &theTime, // supplied C1
    const char *theTag, // supplied C4
    RTI::EventRetractionHandle theHandle) // supplied C1
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateOwnsAttributes,
    RTI::InvalidFederationTime,
    RTI::FederateInternalError) {
    reflectAttributeValues(theObject, theAttributes, theTag);
}

void HwFederateAmbassador::reflectAttributeValues(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleValuePairSet &theAttributes, // supplied C4
    const char *theTag) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateOwnsAttributes,
    RTI::FederateInternalError) {
    ///
    RTI::ULong valueLength = 0;
    string value;
    BridgeHelper::netOffset.x = Utils::Transform::rtiValueToDouble(theAttributes, 0, &value, valueLength);
    BridgeHelper::netOffset.y = Utils::Transform::rtiValueToDouble(theAttributes, 1, &value, valueLength);
}

void HwFederateAmbassador::receiveInteraction(
    RTI::InteractionClassHandle theInteraction, // supplied C1
    const RTI::ParameterHandleValuePairSet &theParameters, // supplied C4
    const RTI::FedTime &theTime, // supplied C4
    const char *theTag, // supplied C4
    RTI::EventRetractionHandle theHandle) // supplied C1
throw(
    RTI::InteractionClassNotKnown,
    RTI::InteractionParameterNotKnown,
    RTI::InvalidFederationTime,
    RTI::FederateInternalError) {
    // call the next service.
    this->receiveInteraction(theInteraction, theParameters, theTag);
}

void HwFederateAmbassador::receiveInteraction(
    RTI::InteractionClassHandle theInteraction, // supplied C1
    const RTI::ParameterHandleValuePairSet &theParameters, // supplied C4
    const char *theTag) // supplied C4
throw(
    RTI::InteractionClassNotKnown,
    RTI::InteractionParameterNotKnown,
    RTI::FederateInternalError) {
    //
    RTI::ULong valueLength = 0;
    string value;

    if (strcmp(theTag, "info_head") == 0) {
        receiveInfoNum = Utils::Transform::rtiValueToInt(theParameters, 0, &value, valueLength);
        sumoSimulation->addDepartedID(Utils::Transform::rtiValueToString(theParameters, 1, &value, valueLength));
        sumoSimulation->addArrivedID(Utils::Transform::rtiValueToString(theParameters, 2, &value, valueLength));
        LOG(INFO) << "新加入车辆：" << Utils::Transform::rtiValueToString(theParameters, 1, &value, valueLength);
    }

    else if (strcmp(theTag, "sumo_car") == 0) {
        SumoActor sumoActor{};

        sumoActor.id = Utils::Transform::rtiValueToString(theParameters, 0, &value, valueLength);
        sumoActor.type = Utils::Transform::rtiValueToString(theParameters, 1, &value, valueLength);
        sumoActor.vClass = Utils::Transform::rtiValueToString(theParameters, 2, &value, valueLength);

        // auto colorVector = Utils::splitStringToInt(
        //     Utils::Transform::rtiValueToString(theParameters, 3, &value, valueLength), ',');
        // sumoActor.color.r = colorVector[0];
        // sumoActor.color.g = colorVector[1];
        // sumoActor.color.b = colorVector[2];
        sumoActor.color = Utils::Transform::rtiValueToString(theParameters, 3, &value, valueLength);

        sumoActor.speed = Utils::Transform::rtiValueToDouble(theParameters, 4, &value, valueLength);

        Position position{};
        position.x = Utils::Transform::rtiValueToDouble(theParameters, 5, &value, valueLength);
        position.y = Utils::Transform::rtiValueToDouble(theParameters, 6, &value, valueLength);
        position.z = Utils::Transform::rtiValueToDouble(theParameters, 7, &value, valueLength);
        sumoActor.position = position;

        sumoActor.angle = Utils::Transform::rtiValueToDouble(theParameters, 8, &value, valueLength);
        sumoActor.slope = Utils::Transform::rtiValueToDouble(theParameters, 9, &value, valueLength);
        sumoActor.length = Utils::Transform::rtiValueToDouble(theParameters, 10, &value, valueLength);
        sumoActor.width = Utils::Transform::rtiValueToDouble(theParameters, 11, &value, valueLength);
        sumoActor.height = Utils::Transform::rtiValueToDouble(theParameters, 12, &value, valueLength);

        sumoSimulation->addSumoActor(sumoActor);

        receiveInfoNumCount++;
        if (receiveInfoNum >= receiveInfoNumCount) {
            receiveInfoNumCount = 0;
        }
    }
}

void HwFederateAmbassador::removeObjectInstance(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::FedTime &theTime, // supplied C4
    const char *theTag, // supplied C4
    RTI::EventRetractionHandle theHandle) // supplied C1
throw(
    RTI::ObjectNotKnown,
    RTI::InvalidFederationTime,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::removeObjectInstance(
    RTI::ObjectHandle theObject, // supplied C1
    const char *theTag) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributesInScope(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributesOutOfScope(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::provideAttributeValueUpdate(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeNotOwned,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnUpdatesOnForObjectInstance(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotOwned,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::turnUpdatesOffForObjectInstance(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotOwned,
    RTI::FederateInternalError) {
}

///////////////////////////////////
// Ownership Management Services //
///////////////////////////////////

void HwFederateAmbassador::requestAttributeOwnershipAssumption(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &offeredAttributes, // supplied C4
    const char *theTag) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeAlreadyOwned,
    RTI::AttributeNotPublished,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipDivestitureNotification(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &releasedAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeNotOwned,
    RTI::AttributeDivestitureWasNotRequested,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipAcquisitionNotification(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &securedAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeAcquisitionWasNotRequested,
    RTI::AttributeAlreadyOwned,
    RTI::AttributeNotPublished,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnershipUnavailable(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeAlreadyOwned,
    RTI::AttributeAcquisitionWasNotRequested,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::requestAttributeOwnershipRelease(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &candidateAttributes, // supplied C4
    const char *theTag) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeNotOwned,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::confirmAttributeOwnershipAcquisitionCancellation(
    RTI::ObjectHandle theObject, // supplied C1
    const RTI::AttributeHandleSet &theAttributes) // supplied C4
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::AttributeAlreadyOwned,
    RTI::AttributeAcquisitionWasNotCanceled,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::informAttributeOwnership(
    RTI::ObjectHandle theObject, // supplied C1
    RTI::AttributeHandle theAttribute, // supplied C1
    RTI::FederateHandle theOwner) // supplied C1
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeIsNotOwned(
    RTI::ObjectHandle theObject, // supplied C1
    RTI::AttributeHandle theAttribute) // supplied C1
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateInternalError) {
}

void HwFederateAmbassador::attributeOwnedByRTI(
    RTI::ObjectHandle theObject, // supplied C1
    RTI::AttributeHandle theAttribute) // supplied C1
throw(
    RTI::ObjectNotKnown,
    RTI::AttributeNotKnown,
    RTI::FederateInternalError) {
}

//////////////////////////////
// Time Management Services //
//////////////////////////////

// 启用时间限制。RTI同意将仿真成员设置为时间管理受限的成员
void HwFederateAmbassador::timeConstrainedEnabled(
    const RTI::FedTime &theFederateTime) // supplied C4
throw(
    RTI::InvalidFederationTime,
    RTI::EnableTimeConstrainedWasNotPending,
    RTI::FederateInternalError) {
    g_currentTime = theFederateTime;
    g_bConstrained = true;
    LOG(TRACE) << "Hw-RTI同意将仿真成员设置为时间管理受限的成员";
}

void HwFederateAmbassador::timeRegulationEnabled(
    const RTI::FedTime &theFederateTime) // supplied C4
throw(
    RTI::InvalidFederationTime,
    RTI::EnableTimeRegulationWasNotPending,
    RTI::FederateInternalError) {
    g_currentTime = theFederateTime;
    g_bRegulation = true;
    LOG(TRACE) << "Hw-RTI同意将仿真成员设置为时间管控成员";
}

// 时间提前授予。RTI同意仿真成员推进到下一步。
void HwFederateAmbassador::timeAdvanceGrant(
    const RTI::FedTime &theTime) // supplied C4
throw(
    RTI::InvalidFederationTime,
    RTI::TimeAdvanceWasNotInProgress,
    RTI::FederateInternalError) {
    LOG(TRACE) << "Hw-RTI同意Carla仿真成员推进到: " << theTime;
    g_currentTime = theTime;
    g_granted = true;
}

void HwFederateAmbassador::requestRetraction(
    RTI::EventRetractionHandle theHandle) // supplied C1
throw(
    RTI::EventNotKnown,
    RTI::FederateInternalError) {
}
